{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Simulation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "<style>\n",
       "    div#notebook-container    { width: 95%; }\n",
       "    div#menubar-container     { width: 65%; }\n",
       "    div#maintoolbar-container { width: 99%; }\n",
       "</style>\n"
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "from IPython.display import Image\n",
    "Image(filename=\"figure.png\",width=600)\n",
    "from IPython.display import display, HTML\n",
    "\n",
    "display(HTML(data=\"\"\"\n",
    "<style>\n",
    "    div#notebook-container    { width: 95%; }\n",
    "    div#menubar-container     { width: 65%; }\n",
    "    div#maintoolbar-container { width: 99%; }\n",
    "</style>\n",
    "\"\"\"))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![gif](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/AerialNavigation/rocket_powered_landing/animation.gif)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Equation generation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy as sp\n",
    "import numpy as np\n",
    "from IPython.display import display\n",
    "sp.init_printing(use_latex='mathjax')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "# parameters\n",
    "# Angular moment of inertia\n",
    "J_B = 1e-2 * np.diag([1., 1., 1.])\n",
    "\n",
    "# Gravity\n",
    "g_I = np.array((-1, 0., 0.))\n",
    "\n",
    "# Fuel consumption\n",
    "alpha_m = 0.01\n",
    "\n",
    "# Vector from thrust point to CoM\n",
    "r_T_B = np.array([-1e-2, 0., 0.])\n",
    "\n",
    "\n",
    "def dir_cosine(q):\n",
    "        return np.matrix([\n",
    "            [1 - 2 * (q[2] ** 2 + q[3] ** 2), 2 * (q[1] * q[2] +\n",
    "                                                   q[0] * q[3]), 2 * (q[1] * q[3] - q[0] * q[2])],\n",
    "            [2 * (q[1] * q[2] - q[0] * q[3]), 1 - 2 *\n",
    "             (q[1] ** 2 + q[3] ** 2), 2 * (q[2] * q[3] + q[0] * q[1])],\n",
    "            [2 * (q[1] * q[3] + q[0] * q[2]), 2 * (q[2] * q[3] -\n",
    "                                                   q[0] * q[1]), 1 - 2 * (q[1] ** 2 + q[2] ** 2)]\n",
    "        ])\n",
    "\n",
    "def omega(w):\n",
    "        return np.matrix([\n",
    "            [0, -w[0], -w[1], -w[2]],\n",
    "            [w[0], 0, w[2], -w[1]],\n",
    "            [w[1], -w[2], 0, w[0]],\n",
    "            [w[2], w[1], -w[0], 0],\n",
    "        ])\n",
    "\n",
    "def skew(v):\n",
    "    return np.matrix([\n",
    "            [0, -v[2], v[1]],\n",
    "            [v[2], 0, -v[0]],\n",
    "            [-v[1], v[0], 0]\n",
    "        ])\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "f = sp.zeros(14, 1)\n",
    "\n",
    "x = sp.Matrix(sp.symbols(\n",
    "    'm rx ry rz vx vy vz q0 q1 q2 q3 wx wy wz', real=True))\n",
    "u = sp.Matrix(sp.symbols('ux uy uz', real=True))\n",
    "\n",
    "g_I = sp.Matrix(g_I)\n",
    "r_T_B = sp.Matrix(r_T_B)\n",
    "J_B = sp.Matrix(J_B)\n",
    "\n",
    "C_B_I = dir_cosine(x[7:11, 0])\n",
    "C_I_B = C_B_I.transpose()\n",
    "\n",
    "f[0, 0] = - alpha_m * u.norm()\n",
    "f[1:4, 0] = x[4:7, 0]\n",
    "f[4:7, 0] = 1 / x[0, 0] * C_I_B * u + g_I\n",
    "f[7:11, 0] = 1 / 2 * omega(x[11:14, 0]) * x[7: 11, 0]\n",
    "f[11:14, 0] = J_B ** -1 * \\\n",
    "    (skew(r_T_B) * u - skew(x[11:14, 0]) * J_B * x[11:14, 0])\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left[\\begin{matrix}- 0.01 \\sqrt{ux^{2} + uy^{2} + uz^{2}}\\\\vx\\\\vy\\\\vz\\\\\\frac{- 1.0 m - ux \\left(2 q_{2}^{2} + 2 q_{3}^{2} - 1\\right) - 2 uy \\left(q_{0} q_{3} - q_{1} q_{2}\\right) + 2 uz \\left(q_{0} q_{2} + q_{1} q_{3}\\right)}{m}\\\\\\frac{2 ux \\left(q_{0} q_{3} + q_{1} q_{2}\\right) - uy \\left(2 q_{1}^{2} + 2 q_{3}^{2} - 1\\right) - 2 uz \\left(q_{0} q_{1} - q_{2} q_{3}\\right)}{m}\\\\\\frac{- 2 ux \\left(q_{0} q_{2} - q_{1} q_{3}\\right) + 2 uy \\left(q_{0} q_{1} + q_{2} q_{3}\\right) - uz \\left(2 q_{1}^{2} + 2 q_{2}^{2} - 1\\right)}{m}\\\\- 0.5 q_{1} wx - 0.5 q_{2} wy - 0.5 q_{3} wz\\\\0.5 q_{0} wx + 0.5 q_{2} wz - 0.5 q_{3} wy\\\\0.5 q_{0} wy - 0.5 q_{1} wz + 0.5 q_{3} wx\\\\0.5 q_{0} wz + 0.5 q_{1} wy - 0.5 q_{2} wx\\\\0\\\\1.0 uz\\\\- 1.0 uy\\end{matrix}\\right]$$"
      ],
      "text/plain": [
       "⎡                                  _________________                          \n",
       "⎢                                 ╱   2     2     2                           \n",
       "⎢                         -0.01⋅╲╱  ux  + uy  + uz                            \n",
       "⎢                                                                             \n",
       "⎢                                     vx                                      \n",
       "⎢                                                                             \n",
       "⎢                                     vy                                      \n",
       "⎢                                                                             \n",
       "⎢                                     vz                                      \n",
       "⎢                                                                             \n",
       "⎢            ⎛    2       2    ⎞                                              \n",
       "⎢-1.0⋅m - ux⋅⎝2⋅q₂  + 2⋅q₃  - 1⎠ - 2⋅uy⋅(q₀⋅q₃ - q₁⋅q₂) + 2⋅uz⋅(q₀⋅q₂ + q₁⋅q₃)\n",
       "⎢─────────────────────────────────────────────────────────────────────────────\n",
       "⎢                                      m                                      \n",
       "⎢                                                                             \n",
       "⎢                              ⎛    2       2    ⎞                            \n",
       "⎢    2⋅ux⋅(q₀⋅q₃ + q₁⋅q₂) - uy⋅⎝2⋅q₁  + 2⋅q₃  - 1⎠ - 2⋅uz⋅(q₀⋅q₁ - q₂⋅q₃)     \n",
       "⎢    ────────────────────────────────────────────────────────────────────     \n",
       "⎢                                     m                                       \n",
       "⎢                                                                             \n",
       "⎢                                                      ⎛    2       2    ⎞    \n",
       "⎢    -2⋅ux⋅(q₀⋅q₂ - q₁⋅q₃) + 2⋅uy⋅(q₀⋅q₁ + q₂⋅q₃) - uz⋅⎝2⋅q₁  + 2⋅q₂  - 1⎠    \n",
       "⎢    ─────────────────────────────────────────────────────────────────────    \n",
       "⎢                                      m                                      \n",
       "⎢                                                                             \n",
       "⎢                     -0.5⋅q₁⋅wx - 0.5⋅q₂⋅wy - 0.5⋅q₃⋅wz                      \n",
       "⎢                                                                             \n",
       "⎢                      0.5⋅q₀⋅wx + 0.5⋅q₂⋅wz - 0.5⋅q₃⋅wy                      \n",
       "⎢                                                                             \n",
       "⎢                      0.5⋅q₀⋅wy - 0.5⋅q₁⋅wz + 0.5⋅q₃⋅wx                      \n",
       "⎢                                                                             \n",
       "⎢                      0.5⋅q₀⋅wz + 0.5⋅q₁⋅wy - 0.5⋅q₂⋅wx                      \n",
       "⎢                                                                             \n",
       "⎢                                      0                                      \n",
       "⎢                                                                             \n",
       "⎢                                   1.0⋅uz                                    \n",
       "⎢                                                                             \n",
       "⎣                                   -1.0⋅uy                                   \n",
       "\n",
       "⎤\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎥\n",
       "⎦"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "display(sp.simplify(f)) # f"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left[\\begin{array}{cccccccccccccc}0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\\\frac{ux \\left(2 q_{2}^{2} + 2 q_{3}^{2} - 1\\right) + 2 uy \\left(q_{0} q_{3} - q_{1} q_{2}\\right) - 2 uz \\left(q_{0} q_{2} + q_{1} q_{3}\\right)}{m^{2}} & 0 & 0 & 0 & 0 & 0 & 0 & \\frac{2 \\left(q_{2} uz - q_{3} uy\\right)}{m} & \\frac{2 \\left(q_{2} uy + q_{3} uz\\right)}{m} & \\frac{2 \\left(q_{0} uz + q_{1} uy - 2 q_{2} ux\\right)}{m} & \\frac{2 \\left(- q_{0} uy + q_{1} uz - 2 q_{3} ux\\right)}{m} & 0 & 0 & 0\\\\\\frac{- 2 ux \\left(q_{0} q_{3} + q_{1} q_{2}\\right) + uy \\left(2 q_{1}^{2} + 2 q_{3}^{2} - 1\\right) + 2 uz \\left(q_{0} q_{1} - q_{2} q_{3}\\right)}{m^{2}} & 0 & 0 & 0 & 0 & 0 & 0 & \\frac{2 \\left(- q_{1} uz + q_{3} ux\\right)}{m} & \\frac{2 \\left(- q_{0} uz - 2 q_{1} uy + q_{2} ux\\right)}{m} & \\frac{2 \\left(q_{1} ux + q_{3} uz\\right)}{m} & \\frac{2 \\left(q_{0} ux + q_{2} uz - 2 q_{3} uy\\right)}{m} & 0 & 0 & 0\\\\\\frac{2 ux \\left(q_{0} q_{2} - q_{1} q_{3}\\right) - 2 uy \\left(q_{0} q_{1} + q_{2} q_{3}\\right) + uz \\left(2 q_{1}^{2} + 2 q_{2}^{2} - 1\\right)}{m^{2}} & 0 & 0 & 0 & 0 & 0 & 0 & \\frac{2 \\left(q_{1} uy - q_{2} ux\\right)}{m} & \\frac{2 \\left(q_{0} uy - 2 q_{1} uz + q_{3} ux\\right)}{m} & \\frac{2 \\left(- q_{0} ux - 2 q_{2} uz + q_{3} uy\\right)}{m} & \\frac{2 \\left(q_{1} ux + q_{2} uy\\right)}{m} & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & - 0.5 wx & - 0.5 wy & - 0.5 wz & - 0.5 q_{1} & - 0.5 q_{2} & - 0.5 q_{3}\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0.5 wx & 0 & 0.5 wz & - 0.5 wy & 0.5 q_{0} & - 0.5 q_{3} & 0.5 q_{2}\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0.5 wy & - 0.5 wz & 0 & 0.5 wx & 0.5 q_{3} & 0.5 q_{0} & - 0.5 q_{1}\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0.5 wz & 0.5 wy & - 0.5 wx & 0 & - 0.5 q_{2} & 0.5 q_{1} & 0.5 q_{0}\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\end{array}\\right]$$"
      ],
      "text/plain": [
       "⎡                                  0                                    0  0  \n",
       "⎢                                                                             \n",
       "⎢                                  0                                    0  0  \n",
       "⎢                                                                             \n",
       "⎢                                  0                                    0  0  \n",
       "⎢                                                                             \n",
       "⎢                                  0                                    0  0  \n",
       "⎢                                                                             \n",
       "⎢   ⎛    2       2    ⎞                                                       \n",
       "⎢ux⋅⎝2⋅q₂  + 2⋅q₃  - 1⎠ + 2⋅uy⋅(q₀⋅q₃ - q₁⋅q₂) - 2⋅uz⋅(q₀⋅q₂ + q₁⋅q₃)         \n",
       "⎢────────────────────────────────────────────────────────────────────   0  0  \n",
       "⎢                                  2                                          \n",
       "⎢                                 m                                           \n",
       "⎢                                                                             \n",
       "⎢                           ⎛    2       2    ⎞                               \n",
       "⎢-2⋅ux⋅(q₀⋅q₃ + q₁⋅q₂) + uy⋅⎝2⋅q₁  + 2⋅q₃  - 1⎠ + 2⋅uz⋅(q₀⋅q₁ - q₂⋅q₃)        \n",
       "⎢─────────────────────────────────────────────────────────────────────  0  0  \n",
       "⎢                                   2                                         \n",
       "⎢                                  m                                          \n",
       "⎢                                                                             \n",
       "⎢                                                 ⎛    2       2    ⎞         \n",
       "⎢2⋅ux⋅(q₀⋅q₂ - q₁⋅q₃) - 2⋅uy⋅(q₀⋅q₁ + q₂⋅q₃) + uz⋅⎝2⋅q₁  + 2⋅q₂  - 1⎠         \n",
       "⎢────────────────────────────────────────────────────────────────────   0  0  \n",
       "⎢                                  2                                          \n",
       "⎢                                 m                                           \n",
       "⎢                                                                             \n",
       "⎢                                  0                                    0  0  \n",
       "⎢                                                                             \n",
       "⎢                                  0                                    0  0  \n",
       "⎢                                                                             \n",
       "⎢                                  0                                    0  0  \n",
       "⎢                                                                             \n",
       "⎢                                  0                                    0  0  \n",
       "⎢                                                                             \n",
       "⎢                                  0                                    0  0  \n",
       "⎢                                                                             \n",
       "⎢                                  0                                    0  0  \n",
       "⎢                                                                             \n",
       "⎣                                  0                                    0  0  \n",
       "\n",
       "0  0  0  0          0                        0                             0  \n",
       "                                                                              \n",
       "0  1  0  0          0                        0                             0  \n",
       "                                                                              \n",
       "0  0  1  0          0                        0                             0  \n",
       "                                                                              \n",
       "0  0  0  1          0                        0                             0  \n",
       "                                                                              \n",
       "                                                                              \n",
       "            2⋅(q₂⋅uz - q₃⋅uy)        2⋅(q₂⋅uy + q₃⋅uz)        2⋅(q₀⋅uz + q₁⋅uy\n",
       "0  0  0  0  ─────────────────        ─────────────────        ────────────────\n",
       "                    m                        m                             m  \n",
       "                                                                              \n",
       "                                                                              \n",
       "                                                                              \n",
       "            2⋅(-q₁⋅uz + q₃⋅ux)  2⋅(-q₀⋅uz - 2⋅q₁⋅uy + q₂⋅ux)       2⋅(q₁⋅ux + \n",
       "0  0  0  0  ──────────────────  ────────────────────────────       ───────────\n",
       "                    m                        m                             m  \n",
       "                                                                              \n",
       "                                                                              \n",
       "                                                                              \n",
       "            2⋅(q₁⋅uy - q₂⋅ux)   2⋅(q₀⋅uy - 2⋅q₁⋅uz + q₃⋅ux)   2⋅(-q₀⋅ux - 2⋅q₂\n",
       "0  0  0  0  ─────────────────   ───────────────────────────   ────────────────\n",
       "                    m                        m                             m  \n",
       "                                                                              \n",
       "                                                                              \n",
       "0  0  0  0          0                     -0.5⋅wx                       -0.5⋅w\n",
       "                                                                              \n",
       "0  0  0  0        0.5⋅wx                     0                           0.5⋅w\n",
       "                                                                              \n",
       "0  0  0  0        0.5⋅wy                  -0.5⋅wz                          0  \n",
       "                                                                              \n",
       "0  0  0  0        0.5⋅wz                   0.5⋅wy                       -0.5⋅w\n",
       "                                                                              \n",
       "0  0  0  0          0                        0                             0  \n",
       "                                                                              \n",
       "0  0  0  0          0                        0                             0  \n",
       "                                                                              \n",
       "0  0  0  0          0                        0                             0  \n",
       "\n",
       "                           0                   0        0        0   ⎤\n",
       "                                                                     ⎥\n",
       "                           0                   0        0        0   ⎥\n",
       "                                                                     ⎥\n",
       "                           0                   0        0        0   ⎥\n",
       "                                                                     ⎥\n",
       "                           0                   0        0        0   ⎥\n",
       "                                                                     ⎥\n",
       "                                                                     ⎥\n",
       " - 2⋅q₂⋅ux)   2⋅(-q₀⋅uy + q₁⋅uz - 2⋅q₃⋅ux)                           ⎥\n",
       "───────────   ────────────────────────────     0        0        0   ⎥\n",
       "                           m                                         ⎥\n",
       "                                                                     ⎥\n",
       "                                                                     ⎥\n",
       "                                                                     ⎥\n",
       "q₃⋅uz)        2⋅(q₀⋅ux + q₂⋅uz - 2⋅q₃⋅uy)                            ⎥\n",
       "──────        ───────────────────────────      0        0        0   ⎥\n",
       "                           m                                         ⎥\n",
       "                                                                     ⎥\n",
       "                                                                     ⎥\n",
       "                                                                     ⎥\n",
       "⋅uz + q₃⋅uy)       2⋅(q₁⋅ux + q₂⋅uy)                                 ⎥\n",
       "────────────       ─────────────────           0        0        0   ⎥\n",
       "                           m                                         ⎥\n",
       "                                                                     ⎥\n",
       "                                                                     ⎥\n",
       "y                       -0.5⋅wz             -0.5⋅q₁  -0.5⋅q₂  -0.5⋅q₃⎥\n",
       "                                                                     ⎥\n",
       "z                       -0.5⋅wy             0.5⋅q₀   -0.5⋅q₃  0.5⋅q₂ ⎥\n",
       "                                                                     ⎥\n",
       "                         0.5⋅wx             0.5⋅q₃   0.5⋅q₀   -0.5⋅q₁⎥\n",
       "                                                                     ⎥\n",
       "x                          0                -0.5⋅q₂  0.5⋅q₁   0.5⋅q₀ ⎥\n",
       "                                                                     ⎥\n",
       "                           0                   0        0        0   ⎥\n",
       "                                                                     ⎥\n",
       "                           0                   0        0        0   ⎥\n",
       "                                                                     ⎥\n",
       "                           0                   0        0        0   ⎦"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "display(sp.simplify(f.jacobian(x)))# A "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left[\\begin{matrix}- \\frac{0.01 ux}{\\sqrt{ux^{2} + uy^{2} + uz^{2}}} & - \\frac{0.01 uy}{\\sqrt{ux^{2} + uy^{2} + uz^{2}}} & - \\frac{0.01 uz}{\\sqrt{ux^{2} + uy^{2} + uz^{2}}}\\\\0 & 0 & 0\\\\0 & 0 & 0\\\\0 & 0 & 0\\\\\\frac{- 2 q_{2}^{2} - 2 q_{3}^{2} + 1}{m} & \\frac{2 \\left(- q_{0} q_{3} + q_{1} q_{2}\\right)}{m} & \\frac{2 \\left(q_{0} q_{2} + q_{1} q_{3}\\right)}{m}\\\\\\frac{2 \\left(q_{0} q_{3} + q_{1} q_{2}\\right)}{m} & \\frac{- 2 q_{1}^{2} - 2 q_{3}^{2} + 1}{m} & \\frac{2 \\left(- q_{0} q_{1} + q_{2} q_{3}\\right)}{m}\\\\\\frac{2 \\left(- q_{0} q_{2} + q_{1} q_{3}\\right)}{m} & \\frac{2 \\left(q_{0} q_{1} + q_{2} q_{3}\\right)}{m} & \\frac{- 2 q_{1}^{2} - 2 q_{2}^{2} + 1}{m}\\\\0 & 0 & 0\\\\0 & 0 & 0\\\\0 & 0 & 0\\\\0 & 0 & 0\\\\0 & 0 & 0\\\\0 & 0 & 1.0\\\\0 & -1.0 & 0\\end{matrix}\\right]$$"
      ],
      "text/plain": [
       "⎡     -0.01⋅ux              -0.01⋅uy              -0.01⋅uz       ⎤\n",
       "⎢────────────────────  ────────────────────  ────────────────────⎥\n",
       "⎢   _________________     _________________     _________________⎥\n",
       "⎢  ╱   2     2     2     ╱   2     2     2     ╱   2     2     2 ⎥\n",
       "⎢╲╱  ux  + uy  + uz    ╲╱  ux  + uy  + uz    ╲╱  ux  + uy  + uz  ⎥\n",
       "⎢                                                                ⎥\n",
       "⎢         0                     0                     0          ⎥\n",
       "⎢                                                                ⎥\n",
       "⎢         0                     0                     0          ⎥\n",
       "⎢                                                                ⎥\n",
       "⎢         0                     0                     0          ⎥\n",
       "⎢                                                                ⎥\n",
       "⎢      2       2                                                 ⎥\n",
       "⎢- 2⋅q₂  - 2⋅q₃  + 1    2⋅(-q₀⋅q₃ + q₁⋅q₂)    2⋅(q₀⋅q₂ + q₁⋅q₃)  ⎥\n",
       "⎢───────────────────    ──────────────────    ─────────────────  ⎥\n",
       "⎢         m                     m                     m          ⎥\n",
       "⎢                                                                ⎥\n",
       "⎢                            2       2                           ⎥\n",
       "⎢ 2⋅(q₀⋅q₃ + q₁⋅q₂)    - 2⋅q₁  - 2⋅q₃  + 1    2⋅(-q₀⋅q₁ + q₂⋅q₃) ⎥\n",
       "⎢ ─────────────────    ───────────────────    ────────────────── ⎥\n",
       "⎢         m                     m                     m          ⎥\n",
       "⎢                                                                ⎥\n",
       "⎢                                                  2       2     ⎥\n",
       "⎢ 2⋅(-q₀⋅q₂ + q₁⋅q₃)    2⋅(q₀⋅q₁ + q₂⋅q₃)    - 2⋅q₁  - 2⋅q₂  + 1 ⎥\n",
       "⎢ ──────────────────    ─────────────────    ─────────────────── ⎥\n",
       "⎢         m                     m                     m          ⎥\n",
       "⎢                                                                ⎥\n",
       "⎢         0                     0                     0          ⎥\n",
       "⎢                                                                ⎥\n",
       "⎢         0                     0                     0          ⎥\n",
       "⎢                                                                ⎥\n",
       "⎢         0                     0                     0          ⎥\n",
       "⎢                                                                ⎥\n",
       "⎢         0                     0                     0          ⎥\n",
       "⎢                                                                ⎥\n",
       "⎢         0                     0                     0          ⎥\n",
       "⎢                                                                ⎥\n",
       "⎢         0                     0                    1.0         ⎥\n",
       "⎢                                                                ⎥\n",
       "⎣         0                    -1.0                   0          ⎦"
      ]
     },
     "execution_count": 10,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sp.simplify(f.jacobian(u)) # B"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Ref\n",
    "\n",
    "- Python implementation of 'Successive Convexification for 6-DoF Mars Rocket Powered Landing with Free-Final-Time' paper\n",
    "by Michael Szmuk and Behçet Açıkmeşe.\n",
    "\n",
    "- inspired by EmbersArc/SuccessiveConvexificationFreeFinalTime: Implementation of \"Successive Convexification for 6-DoF Mars Rocket Powered Landing with Free-Final-Time\" https://github.com/EmbersArc/SuccessiveConvexificationFreeFinalTime\n",
    "\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.8"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
